function  Tu(T1,tc1,tc2,ftxz,tsxz,ftpf,tspf,IK1,IK21,IK22,IK3,T_first,Tool)
%UNTITLED 此处提供此函数的摘要
%   此处提供详细说明
%%
l1=length(T1);
l21=tc1;
l22=tc2;
ftzxj=ftxz;
tsxzj=tsxz;
xzj=zeros(4*l1+2*l21+2*l22-7,1);
ftcha=zeros(l1-1,1);
ftcha(1)=ftzxj(2);
tscha=tsxzj/(155);
for i=1:l21
xzj(i)=i*tscha;
end
for i=1:l1-2
ftcha(i+1)=ftzxj(i+2)-ftzxj(i+1);
end
ftcha=flip(ftcha);
for i=l21+1:l21+l1-1
    xzj(i)=xzj(l21)+sum(ftcha(1:i-l21));
end
ftcha=flip(ftcha);
for i=l21+l1:l21+2*l1-3
    xzj(i)=xzj(l21+l1-1)+sum(ftcha(1:i-l21-l1+1));
end
for i=l21+2*l1-2:2*l21+2*l1-3
    xzj(i)=xzj(l21+2*l1-3)+(i-l21-2*l1+3)*tscha;
end
%加十个点
for i=2*l21+2*l1-2:2*l21+2*l1-2+l22-1
    xzj(i)=xzj(2*l21+2*l1-3)+(i-2*l21-2*l1+3)*tscha;
end

ftcha=flip(ftcha);
for i=2*l21+2*l1-2+l22:2*l21+2*l1-2+l22+44
    xzj(i)=xzj(2*l21+2*l1-2+l22-1)+sum(ftcha(1:i+1-(2*l21+2*l1-2+l22)));
end
ftcha=flip(ftcha);
for i=2*l21+2*l1-2+l22+45:2*l21+2*l1-2+l22+45+43
    xzj(i)=xzj(2*l21+2*l1-2+l22+44)+sum(ftcha(1:i-(2*l21+2*l1-2+l22+45)+1));
end
%加十个点
for i=2*l21+2*l1-2+l22+45+44:2*l21+2*l1-2+l22+45+44+l22-1
    xzj(i)=xzj(2*l21+2*l1-2+l22+45+43)+(i-(2*l21+2*l1-2+l22+45+44))*tscha;
end
xzj=-xzj;

init_pufang=[ftpf(1);ftpf;flip(ftpf);ftpf(1);ftpf;flip(ftpf)];
pufang=zeros(4*l1+2*l21+2*l22-7,1);
pufang(1:l21)=-(90-init_pufang(1));
pufang(l21+1:l21+l1-1)=-(90-init_pufang(3:47));
pufang(l21+l1:l21+2*l1-3)=90-init_pufang(49:92);
pufang(l21+2*l1-2:2*l21+2*l1-3)=90-init_pufang(93);
%加十个点
pufang(2*l21+2*l1-2:2*l21+2*l1-2+l22-1)=90-init_pufang(93);
pufang(2*l21+2*l1-2+l22:2*l21+2*l1-2+l22+44)=90-init_pufang(96:140);
pufang(2*l21+2*l1-2+l22+45:2*l21+2*l1-2+l22+45+43)=-(90-init_pufang(142:185));
%加十个点
pufang(2*l21+2*l1-2+l22+45+44:2*l21+2*l1-2+l22+45+44+l22-1)=-(90-init_pufang(1));

joint=zeros(4*l1+2*l21+2*l22-5,8);
joint(1:l21,1:6)=IK21(:,1:6);
joint(l21+1:l1+l21-1,1:6)=IK3(2:end,1:6);
joint(l1+l21:l21+2*l1-3,1:6)=flip(IK3(2:end-1,1:6));
joint(l21+2*l1-2:2*l21+2*l1-3,1:6)=flip(IK21(1:end,1:6));
%加十个点
joint(2*l21+2*l1-2:2*l21+2*l1-2+l22-1,1:6)=IK22(:,1:6);
joint(2*l21+2*l1-2+l22:2*l21+2*l1-2+l22+44,1:6)=IK1(2:end,1:6);
joint(2*l21+2*l1-2+l22+45:2*l21+2*l1-2+l22+45+43,1:6)=flip(IK1(2:end-1,1:6));

%加十个点
joint(2*l21+2*l1-2+l22+45+44:2*l21+2*l1-2+l22+45+44+l22-1,1:6)=flip(IK22(:,1:6));
joint(1:4*l1+2*l21+2*l22-6,7)=pufang(:,1);
joint(1:4*l1+2*l21+2*l22-6,8)=xzj(:,1); 
joint(2:end,:)=joint(1:4*l1+2*l21+2*l22-6,:);

[joint_first,Error]=my_ik_theta7(T_first,0,Tool);
while Error>9.99999e-10
[joint_first,Error]=my_ik_theta7(T_first,0,Tool);
end
joint(1,:)=[joint_first,-(90-init_pufang(1)),0];
save('joint.mat',"joint");
t1=ceil(linspace(1,l21,16));
t2=ceil(linspace(l21+1,l1+l21-1,10));
t3=ceil(linspace(l1+l21,l21+2*l1-3,9));
t4=ceil(linspace(l21+2*l1-2,2*l21+2*l1-2+l22-1,20));
t5=ceil(linspace(2*l21+2*l1-2+l22,2*l21+2*l1-2+l22+44,10));
t6=ceil(linspace(2*l21+2*l1-2+l22+45,2*l21+2*l1-2+l22+45+43,9));
t7=2*l21+2*l1-2+l22+45+44+l22-1;
t=[0,t1,t2,t3,t4,t5,t6,t7];
t=t+1;
joint=joint(t,:);
save('joint_speed.mat',"joint"); 
end